/*
文件名：pid.cpp
时间：2013-9-11
功能：
	通用pid和简单系统辨识
*/
#include "pid.h"//认为main.h中已经包含了math.h

#define PID		(*p)
//死区PID(带抗饱和积分,积分分离,不完全微分（适合噪声严重的系统）)
float base_pid(float e,PID_CON *p)//传入误差
{
	float tmpu=0;	 //基础输出
	float di=0;
	PID.e=e*PID.k_in + PID.e*(1-PID.k_in); //对误差低通滤波
	if(fabs(PID.e) < PID.die)//死区
	{
		PID.e=0;
		//return PID.u;
	}
	//PID
	PID.up=PID.e * PID.P;
	PID.ui=PID.e_acc;
	PID.ud=PID.ud * (1-PID.Dc) + ((PID.e - PID.e_pre) * PID.D / PID.T) * PID.Dc;//不完全微分
	//小范围加速逼近（P增加:1+2*sqrt(1-(e/K))）
	if(PID.high_e>0.000001) //P变参系数
	{
		tmpu=1-fabs(PID.e)/PID.high_e;
		MINMAX(tmpu,0,1);
		PID.up*=1+2*sqrt(tmpu);
	}
	//d限幅
	MINMAX(PID.ud,-PID.max_d,PID.max_d);
	//准备数据
	PID.e_pre=PID.e;
	tmpu = PID.uf + PID.up + PID.ui + PID.ud;//输出基础+P+I+D
	if(tmpu > PID.max)//限幅+抗饱和积分
	{
		tmpu=PID.max;
		if(e < 0 && e>-PID.E)//积分分离
		{
			di=PID.e*PID.I*PID.T;
		}
	}
	else if(tmpu < PID.min)
	{
		tmpu=PID.min;
		if(e > 0 && e<PID.E)//积分分离
		{
			di=PID.e*PID.I*PID.T;
		}
	}
	else
	{
		if(fabs(e) < PID.E) //积分分离
		{
			di=PID.e*PID.I*PID.T;
		}
	}
	PID.e_acc+=di;
	//积分值限幅（抑制超调）
	MINMAX(PID.e_acc,-PID.max_i,PID.max_i);
	//输出
	PID.u=tmpu;
	return PID.u;
}
////////////////////////////////////////////////////////////////////////////////
//简单辨识：（方程法）此方法只适合在非稳态下做，一旦稳定就无数解了。
//y=(1-k)y + kAmpx    k是系统参数，Amp是增益
//y=Ay + Bx
float srec_run(float x,float y1,SIMP_REC *sr)
{
	return (1-sr->k)*y1 + sr->k*sr->Amp*x;
}
void srec_ini(SIMP_REC *sr)
{
	sr->k=0.17f;//float k; //系统参数
	sr->Amp=2.0f;//float Amp; //增益
	sr->x1=0;//float x1; //前1个值
	sr->y1=0;//float y1; //前1个值
	sr->y2=0;//float y2; //前2个值
	sr->e=0; //误差
	sr->learn_speed=0.2f;//float learn_speed; //学习速度
	sr->stat=0;//int stat; //0为初始化，1为第一个值，2为计算
	sr->err=0;//int err; //错误，0正常，1出错
}
void srec_learn(float x,float y,SIMP_REC *sr)
{
	if(sr->stat==0) //若是初始化
	{
		sr->stat=1;
	}
	else if(sr->stat==1)
	{
		sr->stat=2;
	}
	else if(sr->stat==2)
	{
		sr->stat=1;
		sr->err=0;
		//第二次，开始计算
		//Ay1 + Bx = y
		//Ay2 + Bx1 = y1
		float k,Amp,A,B;
		float y1=sr->y1,y2=sr->y2,x1=sr->x1;
		float yc=srec_run(x,y1,sr); //计算出的结果
		//计算误差
		float e=yc-y;
		sr->e=sqrt((e*e)*2/(y*y + yc*yc));
		float ls=sr->learn_speed*sr->e;
		//float te=1/sr->e;
		//MINMAX(te,0.1f,3);
		//float ls=sr->learn_speed;
		if(sr->e > 0.2f) ls=0.001f;
		MINMAX(ls,0.001f,0.8f);
		if(e<0) sr->e=-sr->e;
		//printf("ls:%.2f\n",ls);
		//Ay1y2 + Bxy2 = yy2
		//Ay2y1 + Bx1y1 = y1y1
		//B=(yy2-y1y1)/(xy2-x1y1)
		float tmp=x*y2-x1*y1;
		if(fabs(tmp)<1e-6)
		{
			sr->err=1;
			goto END;
		}
		B=(y*y2-y1*y1)/tmp;
		if(fabs(y1)<1e-6)
		{
			sr->err=2;
			goto END;
		}
		A=(y - B*x)/y1;
		//printf("	A:%.1f*y1:%.1f + B:%.1f*x:%.1f - y:%.1f\n", A,y1,B,x,y);
		//更新变量
		k=1-A;
		if(k<1e-6 || k>(1-1e-6))
		{
			sr->err=3;
			goto END;
		}
		Amp=B/k;
		if(Amp<=0)
		{
			sr->err=4;
			goto END;
		}
		//更新参数
		sr->k=sr->k*(1-ls) + ls*k;
		sr->Amp=sr->Amp*(1-ls) + ls*Amp;
	}
END:
	//状态变量向后滑动
	sr->y2=sr->y1;
	sr->x1=x; sr->y1=y;
}
////////////////////////////////////////////////////////////////////////////////
//速度辨识：（方程法）此方法只适合在非稳态下做，一旦稳定就无数解了。
//u=(ma + Fs)   500*1 + 200   考虑加速和摩擦力做力平衡方程(摩擦力向后为正)
//u1 = (ma1 + Fs)
//u2 = (ma2 + Fs)
//u2-u1=m(a2-a1)  m=(u2-u1)/(a2-a1) Fs=u1-m*a1
void pull_ini(PULL_REC *sr) //初始化结构
{
	memset(sr,0,sizeof(PULL_REC));
	sr->m=300;
	sr->Fs=200;
	sr->Ts=0.5f;
	sr->learn_speed=0.02f;
}
float pull_run(float u,float v,PULL_REC *sr) //模型运算,输入u输出加速度
{
	float f=v<0?-sr->Fs:sr->Fs;
	f=u-f;
	if(fabs(v)<0.05)//速度很低的时候，静摩擦
	{
		if(fabs(u)<sr->Fs) f=0; //若力矩小于摩擦力
	}
	return f/sr->m; //f为合力
}
void pull_learn(float u,float v,PULL_REC *sr) //模型辨识
{
	float tmp_m,tmp_Fs;
	float t1,t2;
	float a1=sr->a1; //两次数据，做方程解出未知数
	float a2=(v-sr->v1)/sr->Ts;
	float u1=sr->u1;
	float u2=u;
	float v1=sr->v1;
	float v2=v;
	sr->a1=a2;
	sr->v1=v2;
	sr->u1=u2;
	float a=pull_run(u,v,sr); //通过模型求出此时应该的加速度
	sr->e=sr->a1 - a;
	if(v1*t2<0) //若异号,摩擦力变了，求绝对值，消元做加法
	{
		t1=a2 + a1;
		t2=u2 + u1;
	}
	else
	{
		t1=a2 - a1;
		t2=u2 - u1;
	}
	printf("%.1f,%.1f	",sr->m,sr->Fs);
	if(fabs(t1)<0.05f)
	{
		//cout<<"err"<<endl;
		return; //若无法计算
	}
	tmp_m=t2/t1; //质量m
	tmp_Fs = fabs(u1 - tmp_m * a1); //由于摩擦力有负的，这里取绝对值
	if(tmp_m<0) return ;
	//printf("%.1f	%.1f\n",tmp_m,tmp_Fs);
	float e=fabs(sr->e);
	MINMAX(e,0,1); //限制误差在0~1m/s2
	t1=sr->learn_speed*(1-e);
	//t1=sr->learn_speed;
	sr->m = (1-t1)*sr->m + t1*tmp_m;
	sr->Fs = (1-t1)*sr->Fs + t1*tmp_Fs;
}
////////////////////////////////////////////////////////////////////////////////
//牵引辨识：最小二乘法
//用a做方程：F=ma   Ff=f*m
//a=Kx/m - Ff/m
//以半秒的数据做最小二乘，认为半秒内运动状态没有变化，若变化，就是计算值有坏点
void CPULL_REC::run(float u,float v) //模型运算,输入：N，m/s，输出加速度
{
	vbuf[d_ind]=v;
	ubuf[d_ind]=u;
	d_ind++;
	if(d_ind>=ARRAY_SIZE(vbuf)) //若到了计算时刻
	{
		d_ind=0;
		//首先判断计算标准
		//1、若输入为0，或速度为0，则不计算
		float accu=0,accv=0,aveu=0;
		for(int i=0;i<ARRAY_SIZE(ubuf);i++)
		{
			accu+=fabs(ubuf[i]);
			aveu+=ubuf[i];
			accv+=fabs(vbuf[i]);
		}
		accu/=ARRAY_SIZE(ubuf);
		aveu/=ARRAY_SIZE(ubuf);
		accv/=ARRAY_SIZE(ubuf);
		if(accu<10) return ; //还需计算输入的方差，如果输入恒定，就不计算
		//2、若输入的加速度太小，在匀速运动时无法估算k和f，只能计算输出系数
		float ta=(vbuf[ARRAY_SIZE(vbuf)-1]-vbuf[0])/ARRAY_SIZE(vbuf)/Ts;
		if(fabs(ta)<0.05f) //加速过小
		{
			//printf("a=%.2f ",(vbuf[ARRAY_SIZE(vbuf)-1]-vbuf[0])/ARRAY_SIZE(vbuf)/Ts);
			//printf("%.1f-%.1f",vbuf[ARRAY_SIZE(vbuf)-1],vbuf[0]);
			//printf("m:	0	Ff:	%.1f\n",k*accu);
			kf.pd[0][0]=m;
			kf.pd[1][0]=k*aveu*(vbuf[5]>=0?1:-1);
			printf("*");
			goto OUT;
		}
		//首先求加速度序列，用后9个位置
		//ma+Ff=kx
		//Ax=b
		//AtAx=Atb
		//x=(AtA)`Atb
		//其中：b为kx，A为[a,1]
		//x为[m \n Ff]
		for(int i=1;i<ARRAY_SIZE(vbuf);i++) //参与计算的矩阵是少一个
		{
			b.pd[i-1][0]=ubuf[i-1]*k; //u的时间是一一对应的
			A.pd[i-1][1]=vbuf[i]>=0?1:-1;
			if(i>1) A.pd[i-1][0]=(vbuf[i]-vbuf[i-2])/2/Ts; //增加加速度计算跨度，提高精度
			else A.pd[i-1][0]=(vbuf[i]-vbuf[i-1])/Ts; //加速度
			//printf("a:%.3f  v:%.3f  u:%.3f\n",A.pd[i-1][0],vbuf[i],ubuf[i]);
		}
		//PDBG;
		//b.print();
		//PDBG;
		//A.print();
		//PDBG;
		A.t(At);
		//PDBG;
		//At.print();
		//PDBG;
		At.mul(A,AtA);
		AtA.inv(AtA1); //AtA被污染
		At.mul(b,Atb);
		AtA1.mul(Atb,kf);
		//MINMAX(kf.pd[0][0],-0.5f,0.5f);
		MINMAX(kf.pd[0][0],0,1000);
		MINMAX(kf.pd[1][0],-1000,1000);
OUT:
		float fk=0.9f;
		m=m*(1-fk) + fk*kf.pd[0][0];
		Ff=Ff*(1-fk) + fk*kf.pd[1][0];
		printf("a: %.2f	m:	%.1f	Ff:	%.1f\n",ta,m,Ff);
	}
}
